package nl.unimaas.micc.eventSnookerTest.simulation.physics;

import nl.unimaas.micc.eventSnookerTest.domain.Ball;
import nl.unimaas.micc.eventSnookerTest.domain.Ball.BallState;
import nl.unimaas.micc.eventSnookerTest.math.BairstowSolver;
import nl.unimaas.micc.eventSnookerTest.math.Transformation3D;
import nl.unimaas.micc.eventSnookerTest.math.Vector3D;
import nl.unimaas.micc.eventSnookerTest.math.XYRotationTrans3D;

public class Physics
{

	private static class SlidingMotion implements BallMotion
	{
	
		public Vector3D getAngularVelocity(Ball initial, double time)
		{
			Vector3D u = getRelativeVelocity(initial).unit();
			Vector3D av = initial.getAngularVelocity().add(
					new Vector3D(0, 0, 1).crossProduct(u).multiply(
							5.0 * SFRIC * g * (time - initial.getTime())
									/ (2.0 * initial.getRadius())));
			av.z = initial.getAngularVelocity().z - 5.0
			* SPFRIC * g * (time - initial.getTime())
			/ (2.0 * initial.getRadius());
			//System.out.println(av);
			
			return av;
		}
	
		public Vector3D getPosition(Ball initial, double time)
		{
			double ang = -initial.getVelocity().angleTo(new Vector3D(1, 0, 0));	
			Transformation3D tableToBall = new XYRotationTrans3D(ang);
			Transformation3D ballToTable = new XYRotationTrans3D(-ang);

			Vector3D u = getRelativeVelocity(initial, tableToBall).unit();
	
			Vector3D pos = new Vector3D(initial.getVelocity().getLength() * (time - initial.getTime())
					-0.5 * SFRIC * g * (time - initial.getTime()) * (time - initial.getTime()) * u.x,
					-0.5 * SFRIC * g * (time - initial.getTime()) * (time - initial.getTime()) * u.y,
					0);
	
			return initial.getPosition().add(
					ballToTable.transform(pos));
		}
	
		public Vector3D getVelocity(Ball initial, double time)
		{
			double ang = -initial.getVelocity().angleTo(new Vector3D(1, 0, 0));
			Transformation3D tableToBall = new XYRotationTrans3D(ang);
			Transformation3D ballToTable = new XYRotationTrans3D(-ang);
	
			Vector3D u = getRelativeVelocity(initial, tableToBall).unit();
	
			return initial.getVelocity().add(
					ballToTable.transform(u.multiply(-SFRIC * g
							* (time - initial.getTime()))));
		}
	
		public double getDuration(Ball initial)
		{
			Vector3D u = getRelativeVelocity(initial);
			return (2.0 * u.getLength() / (7.0 * SFRIC * g));
		}
	
		public double getFriction(Ball initial)
		{
			return SFRIC;
		}
	
	}

	private static class RollingMotion implements BallMotion
	{
	
		public Vector3D getAngularVelocity(Ball initial, double time)
		{
			// TODO this is just a workaround as nothing but the initial av is queried here.
			return initial.getAngularVelocity();
		}
	
		public Vector3D getPosition(Ball initial, double time)
		{
			return initial.getPosition().add(
					initial.getVelocity().multiply(time - initial.getTime()))
					.subtract(
							initial.getVelocity().unit().multiply(
									0.5 * RFRIC * g
											* (time - initial.getTime())
											* (time - initial.getTime())));
		}
	
		public Vector3D getVelocity(Ball initial, double time)
		{
			return initial.getVelocity().subtract(
					initial.getVelocity().unit().multiply(RFRIC * g * (time-initial.getTime())));
	
		}
	
		public double getDuration(Ball initial)
		{
			return initial.getVelocity().getLength() / (RFRIC * g);
		}
	
		public double getFriction(Ball initial)
		{
			return RFRIC;
		}
	}

	private static class RestingMotion implements BallMotion
	{
	
		public Vector3D getAngularVelocity(Ball initial, double time)
		{
			return Vector3D.nullVector();
		}
	
		public Vector3D getPosition(Ball initial, double time)
		{
			return initial.getPosition();
		}
	
		public Vector3D getVelocity(Ball initial, double time)
		{
			return Vector3D.nullVector();
		}
	
		public double getDuration(Ball initial)
		{
			return Double.POSITIVE_INFINITY;
		}
	
		public double getFriction(Ball initial)
		{
			return 0;
		}
	}

	static
	{
		rollingMotion = new RollingMotion();
		slidingMotion = new SlidingMotion();
		restingMotion = new RestingMotion();
	}

	public static BallMotion getRollingMotion()
	{
		return rollingMotion;
	}

	public static BallMotion getSlidingMotion()
	{
		return slidingMotion;
	}
	
	public static BallMotion getRestingMotion()
	{
		return restingMotion;
	}
	
	public static double collisionTime(Ball initial1, Ball initial2)
	{
		double ang1 = -initial1.getVelocity().angleTo(new Vector3D(1, 0, 0));
		double ang2 = -initial2.getVelocity().angleTo(new Vector3D(1, 0, 0));
		Transformation3D tableToBall1 = new XYRotationTrans3D(ang1);
		Transformation3D tableToBall2 = new XYRotationTrans3D(ang2);

		Vector3D a = Vector3D.nullVector();
		Vector3D b = Vector3D.nullVector();
		
		Vector3D u1;
		Vector3D u2;
		
		if(initial1.getState()==BallState.SLIDING)
		{
			 u1 = getRelativeVelocity(initial1,tableToBall1).unit();
		}
		else
		{
			u1 = tableToBall1.transform(initial1.getVelocity().unit());
		}
		
		if(initial2.getState()==BallState.SLIDING)
		{
			u2 = getRelativeVelocity(initial2,tableToBall2).unit();
		}
		else
		{
			u2 = tableToBall2.transform(initial2.getVelocity().unit());	
		}
		a.x = 0.5 * g * (
						   initial2.getState().getMotion().getFriction(initial2)
						   * (
							   u2.y * Math.sin(ang2)
							  - u2.x * Math.cos(ang2)
							 ) 
					       - initial1.getState().getMotion().getFriction(initial1)
						   * (
							   u1.y * Math.sin(ang1) 
							 - u1.x * Math.cos(ang1)
							 )
						);
		
		a.y = - 0.5 * g * (
			      		    initial2.getState().getMotion().getFriction(initial2)
			      		  * (
			      			   u2.x * Math.sin(ang2) 
			      			 + u2.y * Math.cos(ang2)
			      			) 
			      		  - initial1.getState().getMotion().getFriction(initial1)
			      		  * (
			      		       u1.x * Math.sin(ang1) 
			      		     + u1.y * Math.cos(ang1)
			      		     )
			      		 );
		
		b.x =  initial2.getVelocity().getLength() * Math.cos(ang2)
			  - initial1.getVelocity().getLength() * Math.cos(ang1);
		b.y =  initial2.getVelocity().getLength() * Math.sin(ang2)
			  - initial1.getVelocity().getLength() * Math.sin(ang1);

		Vector3D c = initial2.getPosition().subtract(initial1.getPosition());
		
		double[] co = new double[5];
		
		co[0] = Math.pow(c.x, 2)+Math.pow(c.y, 2)-Math.pow(initial1.getRadius()+initial2.getRadius(), 2);
		co[1] = 2*b.x*c.x+2*b.y*c.y;
		co[2] = Math.pow(b.x, 2) + 2*a.x*c.x + 2*a.y*c.y + Math.pow(b.y, 2);
		co[3] = 2*a.x*b.x + 2*a.y*b.y;
		co[4] = Math.pow(a.x, 2) + Math.pow(a.y, 2);

		solver.setCoefficients(co);
		solver.solve();
		if (solver.foundReal()) {
			return solver.getMinReal();
		}
		return -1;
		
		/*
		co[0] = Math.pow(a.x, 2) + Math.pow(a.y, 2);
		co[1] = 2*a.x*b.x + 2*a.y*b.y;
		co[2] = Math.pow(b.x, 2) + 2*a.x*c.x + 2*a.y*c.y + Math.pow(b.y, 2);
		co[3] = 2*b.x*c.x+2*b.y*c.y;
		co[4] = Math.pow(c.x, 2)+Math.pow(c.y, 2)-Math.pow(initial1.getRadius()+initial2.getRadius(), 2);
		
//		System.out.println(co[0]+" "+co[1]+" "+co[2]+" "+co[3]+" "+co[4]);
		
		BairstowSolver2 solver = new BairstowSolver2(co);
		solver.solve();
		if (solver.foundReal()) {
			return solver.getMinReal();
//			System.out.println(initial1.getPosition().x+","+initial1.getPosition().y);
		}
		return -1;*/
	}
	
	public static Vector3D getRelativeVelocity(Ball ball)
	{
		return ball.getVelocity().add(new Vector3D(0,0,ball.getRadius())
			.crossProduct(ball.getAngularVelocity()));
		
	}
	
	public static Vector3D getRelativeVelocity(Ball ball, Transformation3D t)
	{
		return t.transform(ball.getVelocity()).add(new Vector3D(0,0,ball.getRadius())
			.crossProduct(t.transform(ball.getAngularVelocity())));
	}
	
	private static RollingMotion rollingMotion;
	private static SlidingMotion slidingMotion;
	private static RestingMotion restingMotion;
	
	private static BairstowSolver solver = new BairstowSolver(null,4);

	// ============ Constants ============== \\
	private static final double SFRIC = 0.1;
	private static final double RFRIC = 0.1;
	private static final double SPFRIC = 0.1;
	public static final double g = 9.81;

}
